using System;


// unused in this app
namespace Diversity.Robotics.Navigation.GridSlamApp
{
    /// <summary>
    /// Models gaussians of the form
    /// Y = (1/ ( Sqrt(2*PI*variance)) * e^ -( (X-x)^2 / (2 * variance) )
    /// </summary>
    public class Gaussian : ICloneable
    {
        private double _mean;
        private double _variance;
        private double _factor;
        private double _divisor;

        /// <summary>
        /// Creates a gaussian of the form
        /// Y = (1/ ( Sqrt(2*PI*variance)) * e^ -( (X-x)^2 / (2 * variance) )
        /// </summary>
        /// <param name="mean">The mean</param>
        /// <param name="variance">The square of the Standard Deviation</param>
        public Gaussian(double mean, double variance)
        {
            Mean = mean;
            Variance = variance;
        }

        /// <summary>
        /// The mean of the gaussian
        /// </summary>
        public double Mean
        {
            get { return _mean; }
            set { _mean = value; }
        }

        /// <summary>
        /// The square of the Standard Deviation
        /// </summary>
        public double Variance
        {
            get { return _variance; }
            set
            {
                _variance = value;
                _factor = (1.0 / (Math.Sqrt( _variance * 2 * Math.PI)));
                _divisor = 2 * _variance;
            }
        }

        /// <summary>
        /// The Standard deviation is the square root of the variance
        /// </summary>
        public double StandardDeviation
        {
            get { return Math.Sqrt(_variance); }
            set { Variance = Math.Pow(value, 2); }
        }

        /// <summary>
        /// A measure of the dispertion
        /// </summary>
        public double Entropy
        {
            get { return Math.Log(Math.Sqrt(_variance * 2 * Math.PI * Math.E)); }
        }

        public double Precision
        {
            get {return 1/_variance; }
        }

        public double GetResult(double input)
        {
            return (_factor * Math.Pow(Math.E, -Math.Pow(input - _mean, 2) / _divisor));
        }
        
        public Gaussian Add(Gaussian gaussian)
        {
            double val = _mean + gaussian.Mean;
            double variance = _variance + gaussian.Variance;
            return new Gaussian(val, variance);
        }
        
        public Gaussian KalmanCombine(Gaussian gaussian)
        {
            double combinationFactor = _variance / (gaussian.Variance + _variance);
            double uncertainty = (1 - combinationFactor) * _variance;
            double val = _mean + (combinationFactor * (gaussian.Mean - _mean));
            return new Gaussian(val,uncertainty);
        }
        
        public Object Clone()
        {
            return new Gaussian(_mean, _variance);
        }
    }
}
